/*********************************************************************************************************************
* seekfree_stc32g_wireless_charge_opensource_software 即（STC32G 无线充电开源软件）
* 是一个基于官方 SDK 接口的开源软件
* Copyright (c) 2022 SEEKFREE 逐飞科技
* 
* 本文件是 STC32G 无线充电开源软件的一部分
* 
* STC32G 无线充电开源软件 是免费软件
* 您可以根据自由软件基金会发布的 GPL（GNU General Public License，即 GNU通用公共许可证）的条款
* 即 GPL 的第3版（即 GPL3.0）或（您选择的）任何后来的版本，重新发布和/或修改它
* 
* 本开源库的发布是希望它能发挥作用，但并未对其作任何的保证
* 甚至没有隐含的适销性或适合特定用途的保证
* 更多细节请参见 GPL
* 
* 您应该在收到本开源库的同时收到一份 GPL 的副本
* 如果没有，请参阅<https://www.gnu.org/licenses/>
* 
* 额外注明：
* 本开源库使用 GPL3.0 开源许可证协议 以上许可申明为译文版本
* 许可申明英文版在 libraries/doc 文件夹下的 GPL3_permission_statement.txt 文件中
* 许可证副本在 libraries 文件夹下 即该文件夹下的 LICENSE 文件
* 欢迎各位使用并传播本程序 但修改内容时必须保留逐飞科技的版权声明（即本声明）
* 
* 文件名称          power_sample
* 公司名称          成都逐飞科技有限公司
* 版本信息          查看 libraries/doc 文件夹内 version 文件 版本说明
* 开发环境          MDK 5.37
* 适用平台          STC32G
* 店铺链接          https://seekfree.taobao.com/
* 
* 修改记录
* 日期              作者                备注
* 2023-01-04        Teternal            first version
* 2024-11-28        Teternal            STC32G migration
********************************************************************************************************************/

#include "zf_gpio.h"
#include "zf_adc.h"

#include "charge_control.h"
#include "power_sample.h"

#pragma warning disable = 138
#pragma warning disable = 183
#pragma warning disable = 177

volatile    uint16  charge_display_power = 0;                                   // 显示充电功率
volatile    uint8   charge_battery_cell_count = 0;                              // 锂电池电芯计数

volatile    uint16  charge_power_over_max_count = 0;                            // 充电功率 超标计数
volatile    float   charge_power = 0;                                           // 充电功率
volatile    float   bus_voltage = 0;                                            // 总线电压
volatile    float   bus_current = 0;                                            // 总线电流

static      uint16  bus_voltage_data[CHARGE_ADC_SAMPLES_NUMBER] = {0};              // 总线电压数据缓冲区
static      uint16  bus_current_data[CHARGE_ADC_SAMPLES_NUMBER] = {0};              // 总线电流数据缓冲区
static      float   bus_voltage_smoothing_data[CHARGE_ADC_SMOOTHING_NUMBER] = {0};  // 总线电压数据平滑滤波缓冲区
static      float   bus_current_smoothing_data[CHARGE_ADC_SMOOTHING_NUMBER] = {0};  // 总线电流数据平滑滤波缓冲区

void power_sample_adc_handler (uint16 *voltage_ptr, uint16 *current_ptr, uint16 loop_count)
{
    uint8   register_temp   = 0;
    uint16  loop_temp       = 0;
    uint16  adc_value       = 0;

    register_temp = ADC_CONTR & 0xF0 | CHARGE_ADC_VOLTAGE_CHANNEL;
    ADC_CONTR = register_temp;
    for(loop_temp = 0; loop_temp < loop_count; loop_temp ++)
    {
        ADC_CONTR |= 0x40;                                                      // 启动 AD 转换
        while (!(ADC_CONTR & 0x20));                                            // 查询 ADC 完成标志
        ADC_CONTR &= ~0x20;                                                     // 清完成标志

        adc_value = ADC_RES;                                                    //存储 ADC 的 12 位结果的高 4 位
        adc_value <<= 8;
        adc_value |= ADC_RESL;                                                  //存储 ADC 的 12 位结果的低 8 位

        ADC_RES = 0;
        ADC_RESL = 0;

        *voltage_ptr ++ = adc_value;
    }

    register_temp = ADC_CONTR & 0xF0 | CHARGE_ADC_CURRENT_CHANNEL;
    ADC_CONTR = register_temp;
    for(loop_temp = 0; loop_temp < loop_count; loop_temp ++)
    {
        ADC_CONTR |= 0x40;                                                      // 启动 AD 转换
        while (!(ADC_CONTR & 0x20));                                            // 查询 ADC 完成标志
        ADC_CONTR &= ~0x20;                                                     // 清完成标志

        adc_value = ADC_RES;                                                    //存储 ADC 的 12 位结果的高 4 位
        adc_value <<= 8;
        adc_value |= ADC_RESL;                                                  //存储 ADC 的 12 位结果的低 8 位

        ADC_RES = 0;
        ADC_RESL = 0;

        *current_ptr ++ = adc_value;
    }
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     功率采样 初始电压采样函数
// 参数说明     void
// 返回参数     void
// 使用示例     power_sample_check_battery();
// 备注信息     这段代码会内部调用 这个函数已经过精简优化确保效率
//-------------------------------------------------------------------------------------------------------------------
void power_sample_check_battery (void)
{
    float temp_voltage = 0;
    uint16 temp_count = 0;
    int8 i = 0, j = 0;

    // 连续采样功率部分总线电流和总线电压
//    for(i = 0; CHARGE_ADC_SAMPLES_NUMBER > i;)
//    {
//        bus_voltage_data[i ++] = adc_convert(CHARGE_ADC_VOLTAGE_CHANNEL);
//    }
    power_sample_adc_handler(bus_voltage_data, bus_current_data, CHARGE_ADC_SAMPLES_NUMBER);
    // 冒泡排序 只排序到要使用到的数据
    for(i = 0; CHARGE_ADC_DATA_END >= i; i ++)
    {
        for(j = i + 1; CHARGE_ADC_SAMPLES_NUMBER > j; j ++)
        {
            if(bus_voltage_data[i] < bus_voltage_data[j])
            {
                temp_count = bus_voltage_data[j];
                bus_voltage_data[j] = bus_voltage_data[i];
                bus_voltage_data[i] = temp_count;
            }
        }
    }
    // 将要用的数据累加
    for(i = CHARGE_ADC_DATA_START; CHARGE_ADC_DATA_END >= i; i ++)
    {
        temp_voltage += bus_voltage_data[i];
    }

    // 读取电源电压 电源电压=ADC值/CHARGE_ADC_RESOLUTION_MAX*CHARGE_ADC_REF_VOLTAGE/(10/110)（单位mV）
    temp_voltage = temp_voltage / CHARGE_ADC_USE_DATA_NUMBER * CHARGE_ADC_REF_VOLTAGE * 11.0 / CHARGE_ADC_RESOLUTION_MAX;
#if(CHARGE_VOLTAGE_FACTOR_MAX < 1)
    temp_voltage =  temp_voltage + CHARGE_VOLTAGE_FACTOR_0;
#elif(CHARGE_VOLTAGE_FACTOR_MAX < 2)
    temp_voltage =  temp_voltage * CHARGE_VOLTAGE_FACTOR_1 +
                    CHARGE_VOLTAGE_FACTOR_0;
#elif(CHARGE_VOLTAGE_FACTOR_MAX < 3)
    temp_voltage =  temp_voltage * temp_voltage * CHARGE_VOLTAGE_FACTOR_2 +
                    temp_voltage * CHARGE_VOLTAGE_FACTOR_1 +
                    CHARGE_VOLTAGE_FACTOR_0;
#else
    temp_voltage =  temp_voltage * temp_voltage * temp_voltage * CHARGE_VOLTAGE_FACTOR_3 +
                    temp_voltage * temp_voltage * CHARGE_VOLTAGE_FACTOR_2 +
                    temp_voltage * CHARGE_VOLTAGE_FACTOR_1 +
                    CHARGE_VOLTAGE_FACTOR_0;
#endif

    bus_voltage = temp_voltage;
    for(i = 0; CHARGE_ADC_SMOOTHING_NUMBER > i; i ++)
    {
        bus_voltage_smoothing_data[i] = temp_voltage;
        bus_current_smoothing_data[i] = 0;
    }

    for(charge_battery_cell_count = 1;
            CHARGE_BATTERY_CELLS_VOLTAGE * charge_battery_cell_count < bus_voltage;
            charge_battery_cell_count ++);

    if(3 > charge_battery_cell_count || bus_voltage < CHARGE_VOLTAGE_LOW_LIMIT)
    {
        charge_battery_cell_count = 3;                                          // 最少以 3S 电池计算 避免接入 2S 电池跳过检测后开启发射
        charge_congrol_enable(ZF_DISABLE);
        charge_state = CHARGE_ERR;
    }
    else if(6 < charge_battery_cell_count || bus_voltage > CHARGE_VOLTAGE_HIGH_LIMIT)
    {
        charge_battery_cell_count = 6;                                          // 最多以 6S 电池计算
        charge_congrol_enable(ZF_DISABLE);
        charge_state = CHARGE_ERR;
    }
    else
    {
        if(CHARGE_BATTERY_CELLS_LOW_LIMIT * charge_battery_cell_count > bus_voltage)
        {
            charge_congrol_enable(ZF_DISABLE);                                  // 低于发射启动电压就关闭
            charge_state = CHARGE_LOW_POWER;                                    // 并报错
        }
    }
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     功率采样 功率数据采样函数
// 参数说明     void
// 返回参数     void
// 使用示例     power_sample_handler();
// 备注信息     这个函数已经过精简优化确保效率
//-------------------------------------------------------------------------------------------------------------------
void power_sample_handler (void)
{
    float temp_current = 0;
    float temp_voltage = 0;
    float temp_smoothing_current = 0;
    float temp_smoothing_voltage = 0;
    float temp_power = 0;
    uint16 temp_count = 0;
    int8 i = 0, j = 0;

    // 连续采样功率部分总线电流和总线电压
//    for(i = 0; CHARGE_ADC_SAMPLES_NUMBER > i;)
//    {
//        bus_voltage_data[i   ] = adc_convert(CHARGE_ADC_VOLTAGE_CHANNEL);
//        bus_current_data[i ++] = adc_convert(CHARGE_ADC_CURRENT_CHANNEL);
//    }
    power_sample_adc_handler(bus_voltage_data, bus_current_data, CHARGE_ADC_SAMPLES_NUMBER);
    // 冒泡排序 只排序到要使用到的数据
    for(i = 0; CHARGE_ADC_DATA_END >= i; i ++)
    {
        for(j = i + 1; CHARGE_ADC_SAMPLES_NUMBER > j; j ++)
        {
            if(bus_voltage_data[i] < bus_voltage_data[j])
            {
                temp_count = bus_voltage_data[j];
                bus_voltage_data[j] = bus_voltage_data[i];
                bus_voltage_data[i] = temp_count;
            }
            if(bus_current_data[i] < bus_current_data[j])
            {
                temp_count = bus_current_data[j];
                bus_current_data[j] = bus_current_data[i];
                bus_current_data[i] = temp_count;
            }
        }
    }
    // 将要用的数据累加
    for(i = CHARGE_ADC_DATA_START; CHARGE_ADC_DATA_END >= i; i ++)
    {
        temp_voltage += bus_voltage_data[i];
        temp_current += bus_current_data[i];
    }

    // 读取电源电压 电源电压=ADC值/CHARGE_ADC_RESOLUTION_MAX*CHARGE_ADC_REF_VOLTAGE/(10/110)（单位mV）
    temp_voltage = temp_voltage / CHARGE_ADC_USE_DATA_NUMBER * CHARGE_ADC_REF_VOLTAGE * 11.0 / CHARGE_ADC_RESOLUTION_MAX;

    // 读取总线电流 总线电流=ADC值/CHARGE_ADC_RESOLUTION_MAX*CHARGE_ADC_REF_VOLTAGE/50/CHARGE_SAMPLING_RES（单位A，50是电流检测芯片的放大倍数 CHARGE_SAMPLING_RES是采样电阻x毫欧）
    temp_current = temp_current / CHARGE_ADC_USE_DATA_NUMBER * CHARGE_ADC_REF_VOLTAGE / CHARGE_ADC_RESOLUTION_MAX / 50.0 / CHARGE_SAMPLING_RES;

#if(CHARGE_VOLTAGE_FACTOR_MAX < 1)
    temp_voltage =  temp_voltage + CHARGE_VOLTAGE_FACTOR_0;
#elif(CHARGE_VOLTAGE_FACTOR_MAX < 2)
    temp_voltage =  temp_voltage * CHARGE_VOLTAGE_FACTOR_1 +
                    CHARGE_VOLTAGE_FACTOR_0;
#elif(CHARGE_VOLTAGE_FACTOR_MAX < 3)
    temp_voltage =  temp_voltage * temp_voltage * CHARGE_VOLTAGE_FACTOR_2 +
                    temp_voltage * CHARGE_VOLTAGE_FACTOR_1 +
                    CHARGE_VOLTAGE_FACTOR_0;
#else
    temp_voltage =  temp_voltage * temp_voltage * temp_voltage * CHARGE_VOLTAGE_FACTOR_3 +
                    temp_voltage * temp_voltage * CHARGE_VOLTAGE_FACTOR_2 +
                    temp_voltage * CHARGE_VOLTAGE_FACTOR_1 +
#endif

    temp_smoothing_voltage = temp_voltage;
    temp_smoothing_current = temp_current;
    for(i = CHARGE_ADC_SMOOTHING_NUMBER - 1; 0 < i; i --)
    {
        bus_voltage_smoothing_data[i] = bus_voltage_smoothing_data[i- 1];
        bus_current_smoothing_data[i] = bus_current_smoothing_data[i- 1];

        temp_smoothing_voltage += bus_voltage_smoothing_data[i];
        temp_smoothing_current += bus_current_smoothing_data[i];
    }
    bus_voltage_smoothing_data[0] = temp_voltage;
    bus_current_smoothing_data[0] = temp_current;
    bus_voltage = temp_smoothing_voltage / CHARGE_ADC_SMOOTHING_NUMBER;
    bus_current = temp_smoothing_current / CHARGE_ADC_SMOOTHING_NUMBER;
    // bus_voltage = temp_voltage;
    // bus_current = temp_current;

    // 计算总功率 并进行平滑滤波 单位瓦
    // 首先减去最早的一个功率 然后计算当前功率
    temp_power = bus_voltage * bus_current / 1000.0;
#if(CHARGE_POWER_FACTOR_MAX < 1)
    charge_power =  temp_power + CHARGE_POWER_FACTOR_0;
#elif(CHARGE_POWER_FACTOR_MAX < 2)
    charge_power =  temp_power * CHARGE_POWER_FACTOR_1 +
                    CHARGE_POWER_FACTOR_0;
#elif(CHARGE_POWER_FACTOR_MAX < 3)
    charge_power =  temp_power * temp_power * CHARGE_POWER_FACTOR_2 +
                    temp_power * CHARGE_POWER_FACTOR_1 +
                    CHARGE_POWER_FACTOR_0;
#else
    charge_power =  temp_power * temp_power * temp_power * CHARGE_POWER_FACTOR_3 +
                    temp_power * temp_power * CHARGE_POWER_FACTOR_2 +
                    temp_power * CHARGE_POWER_FACTOR_1 +
                    CHARGE_POWER_FACTOR_0;
#endif

    charge_power = func_limit_ab(charge_power, 0, 600);
    // 插入功率限制保护 当前功率超过最大功率限制 证明当前系统异常 进入错误状态
    if(CHARGE_CLOSE > charge_state && CHARGE_MAX_TARGET < charge_power)
    {
        charge_power_over_max_count ++;
    }
    else
    {
        charge_power_over_max_count = (0 != charge_power_over_max_count) ? (charge_power_over_max_count - 1) : (0);
    }

    if(CHARE_OVER_MAX_POWER_COUNT_MAX < charge_power_over_max_count)
    {
        charge_congrol_enable(ZF_DISABLE);
        charge_power_pidout = CHARGE_STANDBY_PHASE_DIFF;
        charge_state = CHARGE_ERR;
    }

    if(bus_voltage < CHARGE_VOLTAGE_LOW_LIMIT || bus_voltage > CHARGE_VOLTAGE_HIGH_LIMIT)
    {
        charge_congrol_enable(ZF_DISABLE);
        charge_state = CHARGE_ERR;
    }
    if( CHARGE_CLOSE > charge_state &&                                          // 开启发射状态
            CHARGE_BATTERY_CELLS_LOW_LIMIT
        >   bus_voltage / charge_battery_cell_count)                            // 电池电芯电压低于设定值
    {                                                                           // 多数航模电池没有保护板 这里为了防止过放增加保护
        charge_congrol_enable(ZF_DISABLE);
        charge_state = CHARGE_LOW_POWER;
    }
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介    功率采样 初始化
// 参数说明     void
// 返回参数     void
// 使用示例     power_sample_init();
// 备注信息     
//-------------------------------------------------------------------------------------------------------------------
void power_sample_init (void)
{
    uint8   register_temp   = 0;

    P_SW2 |= 0x80;

    if((CHARGE_ADC_VOLTAGE_CHANNEL >> 3) == 1)
    {
        P0M0 &= ~(1 << (CHARGE_ADC_VOLTAGE_CHANNEL & 0x07));
        P0M1 |= (1 << (CHARGE_ADC_VOLTAGE_CHANNEL & 0x07));
    }
    else if((CHARGE_ADC_VOLTAGE_CHANNEL >> 3) == 0)  
    {
        P1M0 &= ~(1 << (CHARGE_ADC_VOLTAGE_CHANNEL & 0x07));
        P1M1 |= (1 << (CHARGE_ADC_VOLTAGE_CHANNEL & 0x07));
    }
    if((CHARGE_ADC_CURRENT_CHANNEL >> 3) == 1)
    {
        P0M0 &= ~(1 << (CHARGE_ADC_CURRENT_CHANNEL & 0x07));
        P0M1 |= (1 << (CHARGE_ADC_CURRENT_CHANNEL & 0x07));
    }
    else if((CHARGE_ADC_CURRENT_CHANNEL >> 3) == 0)  
    {
        P1M0 &= ~(1 << (CHARGE_ADC_CURRENT_CHANNEL & 0x07));
        P1M1 |= (1 << (CHARGE_ADC_CURRENT_CHANNEL & 0x07));
    }

    // ADC 时钟 SYSclk/2 右对齐
    ADC_CONTR |= 1 << 7;
    register_temp = ADCCFG & 0xD0;
    register_temp |= 0x20;
    ADCCFG = register_temp;
}
